
#include <chrono>
#include <rcl/time.h>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/rclcpp.hpp>

#include <rs.hpp>
#include <rsutil.h>
#include <algorithm>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include "cv_msg/msg/rgb_frame.hpp"
#include "QtCore/qsharedmemory.h"


using namespace std::chrono_literals;

static constexpr auto NODE_NAME = ("node_rs");



static auto m_align = rs2::align(RS2_STREAM_COLOR);


class RealSenseCameraNode : public rclcpp::Node
{
public:
	RealSenseCameraNode()
		: Node(NODE_NAME, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
	{
		srand((unsigned)time(NULL));
		RCLCPP_INFO(logger_, "Running with LibRealSense v%s", RS2_API_VERSION_STR);
		publisher_ = this->create_publisher<cv_msg::msg::RgbFrame>("RgbFrame", 10);

		m_smImg = new QSharedMemory("sm_img");
		m_smDepth = new QSharedMemory("sm_depth");

		timer_ = this->create_wall_timer(
			797ms, std::bind(&RealSenseCameraNode::timer_callback, this));
	}

	virtual ~RealSenseCameraNode()
	{}

	virtual void onInit()
	{
		getParameters();
		setupDevice();
		//setupStreams();
		//rclcpp::sleep_for(std::chrono::nanoseconds(2000000000));
		RCLCPP_INFO(logger_, "RealSense Node Is Up!");
	}

private:
	void timer_callback()
	{
		rs2::frameset fs = m_pipeline.wait_for_frames();

		auto frame = m_align.process(fs);
		auto color = frame.get_color_frame();
		auto depth = frame.get_depth_frame();
		auto data_color = color.get_data();
		// RCLCPP_FATAL(logger_, "after data_color %s", m_strSerialNum.c_str());

		// depth pts vertex array
		m_pts = m_ptc.calculate(depth);
		const rs2::vertex* vts = m_pts.get_vertices();

		auto message = cv_msg::msg::RgbFrame();
		message.serial_num = m_strSerialNum;

		if (!m_smImg->isAttached())
		{
			m_smImg->attach();
		}

		if (!m_smDepth->isAttached())
		{
			m_smDepth->attach();
		}

		m_smImg->lock();
		memcpy((char*)m_smImg->data() + 921600 * m_nIndex, data_color, 921600);   // 921600 == 640*480*3
		m_smImg->unlock();

				// RCLCPP_INFO(logger_, "data_color[012]: %d, %d, %d", ((char*)m_smImg->data())[0], ((char*)m_smImg->data())[1], ((char*)m_smImg->data())[2]);

		m_smDepth->lock();
		memcpy((char*)m_smDepth->data() + 3686400 * m_nIndex, vts, 3686400);   // 3686400 == 6400*480*4*3
		m_smDepth->unlock();


		publisher_->publish(message);
		//delete message;
		// RCLCPP_INFO(logger_, "I pub: xxx");
	}


	void setupDevice()
	{
		RCLCPP_INFO(logger_, "setupDevice...");
		try {

			_ctx.reset(new rs2::context());

			bool bFoundDev = false;
			auto list = _ctx->query_devices();
			if (0 == list.size()) {
				_ctx.reset();
				RCLCPP_ERROR(logger_, "No RealSense devices were found! Terminate RealSense Node...");
				rclcpp::shutdown();
				exit(1);
			}
			else
			{
				for (auto dev : list)
				{
					auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
					if (sn == m_strSerialNum)
					{
						bFoundDev = true;
						m_dev = dev;
						_ctx->set_devices_changed_callback([this](rs2::event_information& info)
							{
								if (info.was_removed(m_dev)) {
									RCLCPP_FATAL(logger_, "The device has been disconnected! Terminate RealSense Node...");
									rclcpp::shutdown();
									exit(1);
								}
							});
						break;
					}
				}
			}

			if (!bFoundDev)
			{
				RCLCPP_FATAL(logger_, "Not found device(%s)! Terminate RealSense Node...", m_strSerialNum.c_str());
				rclcpp::shutdown();
				exit(1);
			}
			else
			{
				RCLCPP_FATAL(logger_, "found device(%s)!", m_strSerialNum.c_str());
			}


			m_config.enable_device(m_strSerialNum);
			m_config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 15);
			m_config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGB8, 15);
			m_pipeline.start(m_config);

			//auto dev_sensors = m_dev.query_sensors();
			//for (auto&& elem : dev_sensors) {
			//	std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
			//	if ("Stereo Module" == module_name || "Coded-Light Depth Sensor" == module_name) {
			//		m_depthSensor = std::unique_ptr<rs2::sensor>(new rs2::sensor(elem));

			//		RCLCPP_INFO(logger_, "%s was found.", std::string(elem.get_info(
			//			RS2_CAMERA_INFO_NAME)).c_str());
			//	}
			//	else if ("RGB Camera" == module_name) {
			//		m_colorSensor = std::unique_ptr<rs2::sensor>(new rs2::sensor(elem));

			//		RCLCPP_INFO(logger_, "%s was found.", std::string(elem.get_info(
			//			RS2_CAMERA_INFO_NAME)).c_str());
			//	}
			//}



		}
		catch (const std::exception& ex) {
			RCLCPP_ERROR(logger_, "An exception has been thrown: %s", ex.what());
			throw;
		}
		catch (...) {
			RCLCPP_ERROR(logger_, "Unknown exception has occured!");
			throw;
		}
	}

	void getParameters()
	{
		RCLCPP_INFO(logger_, "getParameters...");

		get_parameter_or(std::string("serial_num"), m_strSerialNum, std::string("no"));
		get_parameter_or(std::string("rs_index"), m_nIndex, 0);

		
		RCLCPP_INFO(logger_, "Device Serial No: %s", m_strSerialNum.c_str());
	}



	void setupStreams()
	{
		//auto frame_callback = [this](rs2::frame frame)
		//{
		//	auto is_color_frame_arrived = false;
		//	auto is_depth_frame_arrived = false;
		//	rs2::frame depth_frame;
		//	if (frame.is<rs2::frameset>()) {
		//		RCLCPP_DEBUG(logger_, "Frameset arrived");
		//		auto frameset = frame.as<rs2::frameset>();
		//		for (auto it = frameset.begin(); it != frameset.end(); ++it) {
		//			auto f = (*it);
		//			auto stream_type = f.get_profile().stream_type();
		//			if (RS2_STREAM_COLOR == stream_type) {
		//				is_color_frame_arrived = true;
		//			}
		//			else if (RS2_STREAM_DEPTH == stream_type) {
		//				depth_frame = f;
		//				is_depth_frame_arrived = true;
		//			}

		//			RCLCPP_DEBUG(logger_,
		//				"Frameset contain %s frame. frame_number: %llu ; frame_TS: %f",
		//				rs2_stream_to_string(stream_type), frame.get_frame_number(),
		//				frame.get_timestamp());
		//			//publishFrame(f, t);
		//		}

		//		if (_align_depth && is_depth_frame_arrived && is_color_frame_arrived) {
		//			RCLCPP_DEBUG(logger_, "publishAlignedDepthTopic(...)");
		//			publishAlignedDepthImg(frame, t);
		//		}

		//		if (_pointcloud && is_depth_frame_arrived && is_color_frame_arrived) {
		//			RCLCPP_DEBUG(logger_, "publishPCTopic(...)");
		//			publishPCTopic(t);
		//		}

		//		if (_align_depth && _align_pointcloud && is_depth_frame_arrived &&
		//			is_color_frame_arrived)
		//		{
		//			RCLCPP_DEBUG(logger_, "publishAlignedPCTopic(...)");
		//			publishAlignedPCTopic(t);
		//		}

		//	}
		//	else {
		//		auto stream_type = frame.get_profile().stream_type();
		//		RCLCPP_DEBUG(logger_,
		//			"%s video frame arrived. frame_number: %llu ; frame_TS: %f",
		//			rs2_stream_to_string(stream_type), frame.get_frame_number(),
		//			frame.get_timestamp());
		//		//publishFrame(frame, t);
		//	}
		//};


		auto profiles = m_depthSensor->get_stream_profiles();
		for (auto& profile : profiles) {
			auto video_profile = profile.as<rs2::video_stream_profile>();
			RCLCPP_INFO(logger_, "format: %d, width: %d, height: %d, fps: %d, stream_index: %d",
				video_profile.format(),
				video_profile.width(),
				video_profile.height(),
				video_profile.fps(),
				video_profile.stream_index());
		}
	}

private:
	std::string m_strSerialNum;
	int m_nIndex;
	rclcpp::Logger logger_ = rclcpp::get_logger(NODE_NAME);
	std::unique_ptr<rs2::context> _ctx;
	rs2::device m_dev;

	rs2::config m_config;
	rs2::pipeline m_pipeline;
	rs2::pointcloud m_ptc;
	rs2::points m_pts;

	std::unique_ptr<rs2::sensor> m_depthSensor;
	std::unique_ptr<rs2::sensor> m_colorSensor;


	rclcpp::TimerBase::SharedPtr timer_;
	rclcpp::Publisher<cv_msg::msg::RgbFrame>::SharedPtr publisher_;

	QSharedMemory* m_smImg;
	QSharedMemory* m_smDepth;

};  // end class


int main(int argc, char* argv[])
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<RealSenseCameraNode>();
	node->onInit();
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
